
function OneTurnLeft()
  nxt.OutputResetTacho(2, 1)
  repeat
    forward(1, -20)
    forward(2, 20)
  until rotationCounts(2) > 50
  forward(1, 0)
  forward(2, 0)
end

function OneTurnRight()
  nxt.OutputResetTacho(1, 1)
  repeat
    forward(1, 20)
    forward(2, -20)
  until rotationCounts(1) > 50
 forward(1, 0)
 forward(2, 0)
 end

function Forward()
  nxt.OutputResetTacho(2, 1)
  repeat
  forward(1, 50)
  forward(2, 50)
  until rotationCounts(2) > 50
  forward(1, 0)
  forward(2, 0)
end

function GetTables()
  width = 3
  numSensors = 3
  funcs = {["FORWARD"] = Forward, ["RIGHT"] = OneTurnRight, ["LEFT"] = OneTurnLeft }
  
  AI = { }
  for j = 1,width + 1 do
      AI[j] = {}
      for k = 1, width+1 do
           AI[j][k] = {}
           for m = 1, numSensors + 1 do
             AI[j][k][m] = 155
	   end
      end
  end
  ActArray = {"FORWARD", "RIGHT", "LEFT"}
  
  TableofStates = {}
  for i = 1, width*width + 1 do
    TableofStates[i] = { ["FORWARD"] = 10, ["RIGHT"] = 10, ["LEFT"] = 10 }
  end
end

function SetStarting()
  LearningRate = 100
  LRConstant = 50
  t = 0
  R = 0
  nextQ = 0
  Nextmax = 0
  Discount = 20

  CurState = 1
  NextState = 1
  epsilon = 40
  nextAction = "RIGHT"
end

function DoAction()
    LearningRate = 100 / ( 1 + t/LRConstant)
    t = t + 1
    Q = 0
    index = 0
    epsilon = epsilon -  1
         print("hirand")
    randi = nxt.random(30, 1)
    print("after")
    if epsilon > randi then

         index = nxt.random(3, 1)
         nextAction = ActArray[index]
         Q = TableofStates[CurState][nextAction]
         
    else

      CurActArray = TableofStates[CurState]
      for i,v in pairs(CurActArray) do
         if v >= Q then
              Q = v
              nextAction = i
         end
      end
    end
    
    funcs[nextAction]()
end

function NSNA()
    NextQ = 0
    for i,v in pairs(TableofStates[NextState]) do
         if v >= NextQ then
              NextQ = v
         end
    end
end

function inRadius(indexX, indexY)
  CurState = CurState - 1
  WX = (CurState * 100 / width)/100 + 1
  WY = CurState % (width) + 1
  radius = ((width*100 / 2) * LearningRate)/10000
  dist = nxt.abs(indexX - WX) + nxt.abs(indexY - WY)
  CurState = CurState + 1
  if dist < radius then
    return true
  else
    return false
  end
  
end 

function TrainingSOM()
  for j = 1, width + 1 do
      for i = 1, width + 1 do
         for k = 1, numSensors + 1 do
	  if inRadius(j, i) then
            AI[j][i][k] = (SonarTable[k] - AI[j][i][k])*LearningRate + AI[j][i][k]
          end
         end      
       end
   end
end

function Directed()
  GetTables()
  SetStarting()
  CurState = GetState()
  repeat
    CurTable = TableofStates[CurDex]
    TrainingSOM()
    DoAction()
    NextState = GetState()
    NSNA()

    DirectedReward()
    R = LearningRate*(Discount * nextQ/100 + reward)
    newQ = ((100 - LearningRate) * Q + R)/100
    TableofStates[CurState][nextAction] = newQ
    CurState = NextState
  until t == 10000
end

function DirectedReward()
    reward = 0
    for i = 1, numSensors + 1 do
      if SonarTable[i] > 25 then
        reward = reward + 25
      end
    end
    if nextAction == "FORWARD" then
      reward = reward + 25
    end
end

function ArraySonar()
  Sensor1 = sonarReading(4)
  Sensor2 = sonarReading(2)
  Sensor3 = sonarReading(3)
  SonarTable = {Sensor1, Sensor2, Sensor3}
end

function GetState(Sensors)
  ArraySonar()

  CloseState = 1000
  Curx = 1
  Cury = 1
  Sum = 0
  for j = 1, width + 1 do
      for i = 1, width + 1 do
        for k = 1, numSensors + 1 do
      	  Sum = Sum + nxt.pow(SonarTable[k] - AI[j][i][1], 2)
      	end
      	Sum = nxt.sqrt(Sum)
      	if Sum <= CloseState then
          CloseState = Sum
          Curx = j
          Cury = i
      	end
      end
   end
   return (Curx-1)*width + Cury
end


